import sensor
import time
import math
import image
import display
from pyb import UART



# 初始化摄像头
def init_camera():
    sensor.reset()
    sensor.set_pixformat(sensor.RGB565)
    sensor.set_framesize(sensor.QQVGA)
    sensor.skip_frames(10)
    sensor.set_auto_whitebal(False)
    return sensor


# 初始化串口
def init_uart(baudrate=115200):
    uart = UART(3, baudrate)
    return uart

# 查找最大Blob
def find_max_blob(blobs):
    max_size = 0
    max_blob = None
    for blob in blobs:
        blob_area = blob[2] * blob[3]
        if blob_area > max_size:
            max_blob = blob
            max_size = blob_area
    return max_blob

# 数据打包函数
def pack_data(x, y):
    # 限制坐标范围
    x = max(0, min(x, 160))
    y = max(0, min(y, 120))  # 假设图像高度为240
    # 打包数据
    data = bytearray([0x2C, 7, x, y, 3, 4, 0x5B])
    return data
    # 格式化字符串协议："X123Y456\n"
    # return "X%03dY%03d\n" % (x, y)  # 固定3位数字长度

# def pack_data(x, y):
#     # 限制坐标范围
#     x = max(0, min(x, 160))
#     y = max(0, min(y, 120))

#     # 十进制格式打包（每个数值占1字节，0-255范围）
#     header = 44   # 0x2C的十进制表示
#     footer = 91    # 0x5B的十进制表示
#     data = bytearray([header, 7, x, y, 3, 4, footer])

#     # 验证十进制转换（可选调试输出）
#     print(f"原始坐标: ({x}, {y})")
#     print(f"数据包十进制值: {list(data)}")

#     return data
#     #return list(data)  # 返回十进制列表 [44,7,100,80,3,4,91]


def draw_closed_polygon(img, points, color=(0,255,0), thickness=2):
    """
    绘制闭合多边形
    :param img: 图像对象
    :param points: 点坐标列表，格式[(x1,y1), (x2,y2), (x3,y3), (x4,y4)]
    :param color: 线条颜色，默认绿色
    :param thickness: 线宽，默认2像素
    """
    # 验证输入有效性
    if len(points) != 4:
        raise ValueError("需要4个点坐标")

    # 连接相邻点
    for i in range(len(points)):
        start_pt = points[i]
        end_pt = points[(i+1)%4]  # 最后一个点连接回第一个点
        img.draw_line(start_pt[0], start_pt[1],
                     end_pt[0], end_pt[1],
                     color=color,
                     thickness=thickness)

def draw_center_cross(img, cx, cy, color=(255,0,0), size=10):
    """绘制中心十字标记"""
    # 水平线
    img.draw_line(cx-size, cy, cx+size, cy, color=color)
    # 垂直线
    img.draw_line(cx, cy-size, cx, cy+size, color=color)

# 主函数
def main():
    # 初始化
    sensor = init_camera()
    uart = init_uart(115200)
    clock = time.clock()
    lcd = display.SPIDisplay()
    threshold = (15, 75, 20, 90, 0, 60)  # 颜色阈值

    #threshold = (15, 75, 20, 90, 0, 60)  # 颜色阈值
    #threshold = (21, 100, -76, 118, -100, 114)
    # 一直拍照
    while(True):
        clock.tick()
        img = sensor.snapshot()#拍摄一张照片，img为一个image对象
        img.lens_corr(1.8)  # 矫正镜头畸变


        # 检测Blob
        blobs = img.find_blobs([threshold])
        if blobs:
            largest_blob = find_max_blob(blobs)
            if largest_blob:
                # 绘制检测结果
                img.draw_rectangle(largest_blob.rect(), color=(255, 0, 0), thickness=2)
                img.draw_cross(largest_blob.cx(), largest_blob.cy(), color=(0, 255, 0), thickness=1)

        # 实际应用场景：从find_rects获取矩形角点
        for r in img.find_rects(threshold=30000):
            if 30 < r.w() < 120 and 30 < r.h() < 120:  # 尺寸过滤
            #if r.w() > 30 and r.h() > 30:
                # 获取四个角点坐标
                corners = r.corners()  # 返回格式：[(x1,y1), (x2,y2), (x3,y3), (x4,y4)]

                # 计算中心坐标（四角点平均值法）
                cx = sum(p[0] for p in corners) // 4
                cy = sum(p[1] for p in corners) // 4

                # 绘制实际检测到的闭合四边形
                draw_closed_polygon(img, corners, color=(255,0,0), thickness=1)

                # 标记角点（可选）
                for p in corners:
                    #img.draw_circle(p[0], p[1], 5, color=(0,0,255))
                    #img.draw_rectangle(r.rect(), color=(0,255,0))  # 外接矩形框
                    draw_center_cross(img, cx, cy, size=max(r.w(),r.h())//8)  # 自适应十字大小
                    lcd.write(img)


            # 打包数据并发送
            try:
                #data = pack_data(r.x(), r.y())
                data = pack_data(cx, cy)
                uart.write(data)
                print("Sent data:", data)
            except Exception as e:
                print("UART write error:", e)
            print(clock.fps())







if __name__ == "__main__":
    main()


